#pragma once
#include <vector>
#include <glm/glm.hpp>
#include <pcl/point_types.h>
#include "pcl/point_cloud.h"
#include "Vertex.hpp"

typedef pcl::PointCloud<pcl::PointXYZRGBNormal> PointXYZCloud;

// class FeatureCloud;

class Registration
{
public:
	
	Registration ();
	~Registration ();
	void registration (std::vector< Vertex > & src,
			std::vector< Vertex > & target, std::vector< Vertex > & dst,
			std::vector< int > &srcNearTargetIndices,
			std::vector< int > &targetNearSrcIndices,
			std::vector< Vertex > & srcNearTargetPoints,
			std::vector< Vertex > & targetNearSrcPoints);

private:

	void _glmToPcl (std::vector< Vertex > &input, PointXYZCloud::Ptr output);
	void _pclToGlm (PointXYZCloud::Ptr input, std::vector< Vertex > &output);
	Eigen::Matrix4f _doAlign (PointXYZCloud::Ptr srcCloud, PointXYZCloud::Ptr targetCloud);
// 	Eigen::Matrix4f _doPPF (PointXYZCloud::Ptr srcCloud, PointXYZCloud::Ptr targetCloud);
// 	Eigen::Matrix4f _doSAC (FeatureCloud *src, FeatureCloud *target);
	void _getNearTargetPoints (PointXYZCloud::Ptr srcCloud,
			PointXYZCloud::Ptr targetCloud, PointXYZCloud::Ptr srcNearTargetCloud,
			std::vector< int > &resultIndices);
	void _cutCloud (PointXYZCloud::Ptr src, PointXYZCloud::Ptr result);
	void _downSampling (PointXYZCloud::Ptr srcCloud, PointXYZCloud::Ptr resultCloud, float step);
};